#pragma once
#include "DBody.h"
#include "DOde.h"
#include "DWorld.h"
#include "DMatrix3.h"
#include "DVector3.h"
#include "DQuaternion.h"
#include "DMass.h"
#include "Joints/DJoint.h"
#include "Geometries/DGeometry.h"

void Dbody_Callback(dBodyID body)
{
	if(body)
	{
		Skill::Ode::DIRegisteredObject^ p = Skill::Ode::DRegisteredObjects::Get(IntPtr(body));
		if(p)
		{
			Skill::Ode::DBody^ Body= dynamic_cast<Skill::Ode::DBody^>(p);
			if(Body)
				Body->Raise_Move();
		}
	}
}

namespace Skill
{
	namespace Ode
	{		
		void DBody::CollectManagedMemory()
		{
			if(_World)
				_World->RemoveBody(this);
			_World = nullptr;
			_UserData = nullptr;
			_Position = nullptr;
			_Rotation = nullptr;
			_Quaternion = nullptr;
			_LinearVelocity = nullptr;
			_AngularVelocity = nullptr;
			_Mass = nullptr;
			_Force = nullptr;
			_Torque = nullptr;
			_Name = nullptr;
			_FiniteRotationAxis = nullptr;
		}
		DBody::DBody(DWorld^ world)
		{
			dWorldID w = world ? world->_This : NULL;
			_World = world;
			_SetPointer(dBodyCreate(w),true);
			dBodySetMovedCallback(_This,Dbody_Callback);
			if(_World)
				_World->AddBody(this);
		}
		DBody::!DBody()
		{			
			if(_Free && _This && DOde::IsInitialized)
				dBodyDestroy(_This);
			_This = nullptr;
			_Free = false;
			_Disposed = true;
		}

		String^ DBody::Name::get()
		{
			return _Name;
		}
		void DBody::Name::set(String^ value)
		{
			_Name = value;
		}

		dReal DBody::AutoDisableLinearThreshold::get()
		{
			return dBodyGetAutoDisableLinearThreshold(_This);
		}
		void DBody::AutoDisableLinearThreshold::set(dReal value)
		{
			dBodySetAutoDisableLinearThreshold(_This,value);
		}

		dReal DBody::AutoDisableAngularThreshold::get()
		{
			return dBodyGetAutoDisableAngularThreshold(_This);
		}
		void DBody::AutoDisableAngularThreshold::set(dReal value)
		{
			dBodySetAutoDisableAngularThreshold(_This,value);
		}

		int DBody::AutoDisableAverageSamplesCount::get()
		{
			return dBodyGetAutoDisableAverageSamplesCount(_This);
		}
		void DBody::AutoDisableAverageSamplesCount::set(int value)
		{
			dBodySetAutoDisableAverageSamplesCount(_This,value);
		}

	
		int DBody::AutoDisableSteps::get()
		{
			return dBodyGetAutoDisableSteps(_This);
		}
		void DBody::AutoDisableSteps::set(int value)
		{
			dBodySetAutoDisableSteps(_This,value);
		}
		
		dReal DBody::AutoDisableTime::get()
		{
			return dBodyGetAutoDisableTime(_This);
		}
		void DBody::AutoDisableTime::set(dReal value)
		{
			dBodySetAutoDisableTime(_This,value);
		}

		int DBody::AutoDisableFlag::get()
		{
			return dBodyGetAutoDisableFlag(_This);
		}
		void DBody::AutoDisableFlag::set(int value)
		{
			dBodySetAutoDisableFlag(_This,value);
		}

		void DBody::SetAutoDisableDefaults()
		{
			dBodySetAutoDisableDefaults(_This);
		}

		DWorld^ DBody::World::get()
		{
			return _World;
		}

		Object^ DBody::UserData::get()
		{
			return _UserData;
		}
		void DBody::UserData::set(Object^ value)
		{
			_UserData = value;
		}

		DVector3^ DBody::Position::get()
		{
			if(!_Position)
				_Position = gcnew DVector3();
			dBodyCopyPosition(_This,_Position->_This);
			return _Position;
		}
		void DBody::Position::set(DVector3^ value)
		{
			dBodySetPosition(_This,value->_This[0],value->_This[1],value->_This[2]);
		}

		DMatrix3^ DBody::Rotation::get()
		{
			if(!_Rotation)
				_Rotation = gcnew DMatrix3();
			dBodyCopyRotation(_This,_Rotation->_This);
			return _Rotation;
		}
		void DBody::Rotation::set(DMatrix3^ value)
		{
			dBodySetRotation(_This,value->_This);
		}

		DMatrix3^ DBody::WorldMatrix::get()
		{			
			if(!_WorldMatrix)
				_WorldMatrix = gcnew DMatrix3();
			dBodyCopyRotation(_This,_WorldMatrix->_This);
			const dReal* p = dBodyGetPosition(_This);
			_WorldMatrix->_This[3]  = p[0];
			_WorldMatrix->_This[7]  = p[1];
			_WorldMatrix->_This[11] = p[2];
			return _WorldMatrix;
		}
		void DBody::WorldMatrix::set(DMatrix3^ value)
		{							
			dBodySetRotation(_This,value->_This);
			dBodySetPosition(_This,value->_This[3],value->_This[7],value->_This[11]);
		}

		DQuaternion^ DBody::Quaternion::get()
		{
			if(!_Quaternion)
				_Quaternion = gcnew DQuaternion();
			dBodyCopyQuaternion(_This,_Quaternion->_This);
			return _Quaternion;
		}
		void DBody::Quaternion::set(DQuaternion^ value)
		{
			dBodySetQuaternion(_This,value->_This);
		}

		DVector3^ DBody::LinearVelocity::get()
		{
			if(!_LinearVelocity)
				_LinearVelocity = gcnew DVector3();
			const dReal* l = dBodyGetLinearVel(_This);
			_LinearVelocity->_This[0] = l[0];
			_LinearVelocity->_This[1] = l[1];
			_LinearVelocity->_This[2] = l[2];
			return _LinearVelocity;
		}
		void DBody::LinearVelocity::set(DVector3^ value)
		{
			dBodySetLinearVel(_This,value->_This[0],value->_This[1],value->_This[2]);
		}

		DVector3^ DBody::AngularVelocity::get()
		{
			if(!_AngularVelocity)
				_AngularVelocity = gcnew DVector3();
			const dReal* a = dBodyGetAngularVel(_This);
			_AngularVelocity->_This[0] = a[0];
			_AngularVelocity->_This[1] = a[1];
			_AngularVelocity->_This[2] = a[2];
			return _AngularVelocity;
		}
		void DBody::AngularVelocity::set(DVector3^ value)
		{
			dBodySetAngularVel(_This,value->_This[0],value->_This[1],value->_This[2]);
		}

		DMass^ DBody::Mass::get()
		{
			dMass m;
			dBodyGetMass(_This,&m);

			if(!_Mass)							
				_Mass = gcnew DMass();			
			*_Mass->_This = m;
			return _Mass;						
		}
		void DBody::Mass::set(DMass^ value)
		{
			dBodySetMass(_This,value->_This);			
		}

		void DBody::AddForce(dReal fx, dReal fy, dReal fz)
		{
			dBodyAddForce(_This,fx,fy,fz);
		}
		void DBody::AddForce(DVector3^ f)
		{
			dBodyAddForce(_This,f->_This[0],f->_This[1],f->_This[2]);
		}
		void DBody::AddTorque(dReal fx, dReal fy, dReal fz)
		{
			dBodyAddTorque(_This,fx,fy,fz);
		}
		void DBody::AddTorque(DVector3^ f)
		{
			dBodyAddTorque(_This,f->_This[0],f->_This[1],f->_This[2]);
		}
		void DBody::AddRelativeForce(dReal fx, dReal fy, dReal fz)
		{
			dBodyAddRelForce(_This,fx,fy,fz);
		}
		void DBody::AddRelativeForce(DVector3^ f)
		{
			dBodyAddRelForce(_This,f->_This[0],f->_This[1],f->_This[2]);
		}
		void DBody::AddRelativeTorque(dReal fx, dReal fy, dReal fz)
		{
			dBodyAddRelTorque(_This,fx,fy,fz);
		}
		void DBody::AddRelativeTorque(DVector3^ f)
		{
			dBodyAddRelTorque(_This,f->_This[0],f->_This[1],f->_This[2]);
		}

		void DBody::AddForceAtPosition(dReal fx, dReal fy, dReal fz,
			dReal px, dReal py, dReal pz)
		{
			dBodyAddForceAtPos(_This,fx,fy,fz,px,py,pz);
		}
		void DBody::AddForceAtPosition(DVector3^ f,DVector3^ p)
		{
			dBodyAddForceAtPos(_This,f->_This[0],f->_This[1],f->_This[2] , p->_This[0],p->_This[1],p->_This[2]);
		}

		void DBody::AddForceAtRelativePosition(dReal fx, dReal fy, dReal fz,
			dReal px, dReal py, dReal pz)
		{
			dBodyAddForceAtRelPos(_This,fx,fy,fz,px,py,pz);
		}
		void DBody::AddForceAtRelativePosition(DVector3^ f,DVector3^ p)
		{
			dBodyAddForceAtRelPos(_This,f->_This[0],f->_This[1],f->_This[2] , p->_This[0],p->_This[1],p->_This[2]);
		}

		void DBody::AddRelativeForceAtPosition(dReal fx, dReal fy, dReal fz,
			dReal px, dReal py, dReal pz)
		{
			dBodyAddRelForceAtPos(_This,fx,fy,fz,px,py,pz);
		}
		void DBody::AddRelativeForceAtPosition(DVector3^ f,DVector3^ p)
		{
			dBodyAddRelForceAtPos(_This,f->_This[0],f->_This[1],f->_This[2] , p->_This[0],p->_This[1],p->_This[2]);
		}		
		void DBody::AddRelativeForceAtRelativePosition(dReal fx, dReal fy, dReal fz,
			dReal px, dReal py, dReal pz)
		{
			dBodyAddRelForceAtRelPos(_This,fx,fy,fz,px,py,pz);
		}
		void DBody::AddRelativeForceAtRelativePosition(DVector3^ f,DVector3^ p)
		{
			dBodyAddRelForceAtRelPos(_This,f->_This[0],f->_This[1],f->_This[2] , p->_This[0],p->_This[1],p->_This[2]);
		}

		DVector3^ DBody::Force::get()
		{
			if(!_Force)
				_Force = gcnew DVector3();
			const dReal* l = dBodyGetForce(_This);
			_Force->_This[0] = l[0];
			_Force->_This[1] = l[1];
			_Force->_This[2] = l[2];
			return _Force;
		}
		void DBody::Force::set(DVector3^ value)
		{
			dBodySetForce(_This,value->_This[0],value->_This[1],value->_This[2]);
		}

		DVector3^ DBody::Torque::get()
		{
			if(!_Torque)
				_Torque = gcnew DVector3();
			const dReal* l = dBodyGetTorque(_This);
			_Torque->_This[0] = l[0];
			_Torque->_This[1] = l[1];
			_Torque->_This[2] = l[2];
			return _Torque;
		}
		void DBody::Torque::set(DVector3^ value)
		{
			dBodySetTorque(_This,value->_This[0],value->_This[1],value->_This[2]);
		}


		void DBody::GetRelativePointPosition(dReal px, dReal py, dReal pz, DVector3^ result)
		{
			dBodyGetRelPointPos(_This,px,py,pz,result->_This);
		}
		void DBody::GetRelativePointPosition(DVector3^ p, DVector3^ result)
		{
			dBodyGetRelPointPos(_This,p->_This[0],p->_This[1],p->_This[2],result->_This);
		}

		void DBody::GetRelativePointVelocity(dReal px, dReal py, dReal pz,	DVector3^ result)
		{
			dBodyGetRelPointVel(_This,px,py,pz,result->_This);
		}
		void DBody::GetRelativePointVelocity(DVector3^ p,	DVector3^ result)
		{
			dBodyGetRelPointVel(_This,p->_This[0],p->_This[1],p->_This[2],result->_This);
		}
		
		void DBody::GetPointVelocity(dReal px, dReal py, dReal pz, DVector3^ result)
		{
			dBodyGetPointVel(_This,px,py,pz,result->_This);
		}
		void DBody::GetPointVelocity(DVector3^ p, DVector3^ result)
		{
			dBodyGetPointVel(_This,p->_This[0],p->_This[1],p->_This[2],result->_This);
		}

		void DBody::GetPositionRelativePoint(dReal px, dReal py, dReal pz,DVector3^ result)
		{
			dBodyGetPosRelPoint(_This,px,py,pz,result->_This);
		}
		void DBody::GetPositionRelativePoint(DVector3^ p,DVector3^ result)
		{
			dBodyGetPosRelPoint(_This,p->_This[0],p->_This[1],p->_This[2],result->_This);
		}
		
		void DBody::VectorToWorld(dReal px, dReal py, dReal pz,DVector3^ result)
		{
			dBodyVectorToWorld(_This,px,py,pz,result->_This);
		}
		void DBody::VectorToWorld(DVector3^ p,DVector3^ result)
		{
			dBodyVectorToWorld(_This,p->_This[0],p->_This[1],p->_This[2],result->_This);
		}
		
		void DBody::VectorFromWorld(dReal px, dReal py, dReal pz,DVector3^ result)
		{
			dBodyVectorFromWorld(_This,px,py,pz,result->_This);
		}
		void DBody::VectorFromWorld(DVector3^ p,DVector3^ result)
		{
			dBodyVectorFromWorld(_This,p->_This[0],p->_This[1],p->_This[2],result->_This);
		}

		int DBody::FiniteRotationMode::get()
		{
			return dBodyGetFiniteRotationMode(_This);
		}
		void DBody::FiniteRotationMode::set(int value)
		{
			dBodySetFiniteRotationMode(_This,value);
		}
		
		DVector3^ DBody::FiniteRotationAxis::get()
		{
			if(!_FiniteRotationAxis)
				_FiniteRotationAxis = gcnew DVector3();
			dBodyGetFiniteRotationAxis(_This,_FiniteRotationAxis->_This);
			return _FiniteRotationAxis;
		}
		void DBody::FiniteRotationAxis::set(DVector3^ value)
		{
			dBodySetFiniteRotationAxis(_This,value->_This[0],value->_This[1],value->_This[2]);
		}

		int DBody::NumJoints::get()
		{
			return dBodyGetNumJoints(_This);
		}

		DJoint^ DBody::GetJoint(int index)
		{
			dJointID j = dBodyGetJoint(_This,index);
			if(j)
			{
				DIRegisteredObject^ p = DRegisteredObjects::Get(IntPtr(j));
				if(p)
					return dynamic_cast<DJoint^>(p);
			}
			return nullptr;
		}

		void DBody::Enable()
		{
			dBodyEnable(_This);
		}
		void DBody::Disable()
		{
			dBodyDisable(_This);
		}

		bool DBody::IsEnabled::get()
		{
			return dBodyIsEnabled(_This) == 1;
		}

		int DBody::GravityMode::get()
		{
			return dBodyGetGravityMode(_This);
		}
		void DBody::GravityMode::set(int value)
		{
			dBodySetGravityMode(_This,value);
		}
		DGeometry^ DBody::FirstGeometry::get()
		{
			dGeomID g = dBodyGetFirstGeom(_This);
			if(g)
			{
				DIRegisteredObject^ p = DRegisteredObjects::Get(IntPtr(g));
				if(p)
					return dynamic_cast<DGeometry^>(p);
			}
			return nullptr;
		}
		DGeometry^ DBody::GetNextGeometry(DGeometry^ g)
		{
			if(g)
			{
				dGeomID g1 = dBodyGetNextGeom(g->_Geom());
				if(g1)
				{
					DIRegisteredObject^ p = DRegisteredObjects::Get(IntPtr(g1));
					if(p)
						return dynamic_cast<DGeometry^>(p);
				}			
			}
			return nullptr;
		}
		void DBody::SetDampingDefaults()
		{
			dBodySetDampingDefaults(_This);
		}

		dReal DBody::LinearDamping::get()
		{
			return dBodyGetLinearDamping(_This);
		}
		void DBody::LinearDamping::set(dReal value)
		{
			dBodySetLinearDamping(_This,value);
		}

		dReal DBody::AngularDamping::get()
		{
			return dBodyGetAngularDamping(_This);
		}
		void DBody::AngularDamping::set(dReal value)
		{
			dBodySetAngularDamping(_This,value);
		}
		void DBody::SetDamping(dReal linearScale, dReal angularScale)
		{
			dBodySetDamping(_This ,linearScale, angularScale);
		}

		dReal DBody::LinearDampingThreshold::get()
		{
			return dBodyGetLinearDampingThreshold(_This);
		}
		void DBody::LinearDampingThreshold::set(dReal value)
		{
			dBodySetLinearDampingThreshold(_This,value);
		}
		dReal DBody::AngularDampingThreshold::get()
		{
			return dBodyGetAngularDampingThreshold(_This);
		}
		void DBody::AngularDampingThreshold::set(dReal value)
		{
			dBodySetAngularDampingThreshold(_This,value);
		}

		dReal DBody::MaxAngularSpeed::get()
		{
			return dBodyGetMaxAngularSpeed(_This);
		}
		void DBody::MaxAngularSpeed::set(dReal value)
		{
			dBodySetMaxAngularSpeed(_This,value);
		}
	}
}